/**
 * @copyright (C) COPYRIGHT 2022 Fortiortech Shenzhen
 * @file      xxx.c
 * @author    Fortiortech  Appliction Team
 * @date      2022-09-13
 * @brief     This file contains   function used for Motor Control.
 */

#include <MyProject.h>

#if (TAILWIND_MODE == BEMFMethod)

uint8 code table_Bemf_CWNext[6] = {3, 6, 2, 5, 1, 4};
uint8 code table_Bemf_CWPre[6] = {5, 3, 1, 6, 4, 2};

BEMFDetect_TypeDef xdata mcBemf;

static void Cmp_Bemf_Init(void);
static uint8 FR_Detect(uint8 bemfStatus, uint8 setFr);
static void Time1_Bemf_Init(void);
void BEMFFOCCloseLoopStart(void);

/**
 * @brief        Bemf的顺逆风检测，比较器硬件初始化，注意需要核对IO是否对应
 * @date         2022-07-09
 */
static void Cmp_Bemf_Init(void)
{
    /*  -------------------------------------------------------------------------------------------------
        CMP Input Pin Mode
        0: GPIO Mode,   P1.4--CMP0_IN+, P1.6--CMP1_IN+, P2.1--CMP2_IN+
                        P1.5--CMP0_IN-, P1.7--CMP1_IN-, P2.2--CMP2_IN-
        1: BEMF Mode,   比较器正端连接到内部星型连接电阻U、V、W的BMEF采样点，
                        比较器负端连接到内部星型连接电阻的虚拟中性点
                        比较器负端与P1.5/P1.7/P2.2断开，这三个GPIO可做其他用途
        -------------------------------------------------------------------------------------------------*/
    SetBit(P1_AN, P14 | P16); // CMP0/1 Pin设置为模拟模式  +
    SetBit(P2_AN, P21);       // CMP2 Pin设置为模拟模式  +
    ClrBit(P1_PU, P14);       // P14上拉关闭
    /*  -------------------------------------------------------------------------------------------------
        CMP0_MOD：
        00：  无内置虚拟中心点电阻的BEMF模式
        01：  内置虚拟中心点电阻的BEMF模式
        10：  3差分比较器模式
        11：  2比较器模式
        -------------------------------------------------------------------------------------------------*/
    SetReg(CMP_CR2, CMP0MOD0 | CMP0MOD1, CMP0MOD0);
    /*  -------------------------------------------------------------------------------------------------
        比较器输出选择配置，与CMP0_MOD配合使用
        CMP0_SEL[1:0]=00，比较器0工作在3比较器轮询模式，正端在CMP0P、CMP1P、CMP2P之间自动轮流选择，
                      负端固定接内置BEMF电阻的中心点，其输出结果分别送至CMP0_OUT、CMP1_OUT、CMP2_OUT
        CMP0_SEL[1:0]=01，比较器0选择CMP0对应的端口组合，正端接CMP0P，负端接内置BEMF电阻的中心点，输出接CMP0_OUT
        CMP0_SEL[1:0]=10，比较器0选择CMP1对应的端口组合，正端接CMP1P，负端接内置BEMF电阻的中心点，输出接CMP1_OUT
        CMP0_SEL[1:0]=11，比较器0选择CMP2对应的端口组合，正端接CMP2P，负端接内置BEMF电阻的中心点，输出接CMP2_OUT
        -----------------------------------------------------------------------------*/
    SetReg(CMP_CR2, CMP0SEL0 | CMP0SEL1, 0x00);
    /*  -------------------------------------------------------------------------------------------------
        比较器迟滞电压选择
        000: 无迟滞   001: ±2.5mV   010: -5mV   011: +5mV
        100: +-5mV   101: -10mV   110: +10mV   111: +-10mV
        -------------------------------------------------------------------------------------------------*/
    SetReg(CMP_CR1, CMP0HYS0 | CMP0HYS1, CMP0HYS0);
    /*  -------------------------------------------------------------------------------------------------
        CMP0的轮询时间设置
        -------------------------------------------------------------------------------------------------*/
    SetReg(CMP_CR2, CMP0CSEL1 | CMP0CSEL0, 0x00);
    /*  -------------------------------------------------------------------------------------------------
        比较器中断模式配置 CMP_CR0[CMP2IM1], CMP_CR0[CMP2IM0]
        00: 不产生中断  01: 上升沿产生中断  10: 下降沿产生中断  11: 上升/下降沿产生中断
        -------------------------------------------------------------------------------------------------*/
    SetReg(CMP_CR0, CMP2IM0 | CMP2IM1, 0);
    SetReg(CMP_CR0, CMP1IM0 | CMP1IM1, 0);
    SetReg(CMP_CR0, CMP0IM0 | CMP0IM1, 0);
    ClrBit(CMP_CR4, CMP0_FS); // CMP1/2功能转移  仅CMP0_MOD=01时有效
    SetBit(CMP_CR2, CMP0EN);  // 开三个比较器
}

/**
 * @brief        Timer1 初始化函数
 * @date         2022-07-09
 */
static void Time1_Bemf_Init(void)
{
    ClrBit(TIM1_CR0, T1BCEN);
    /* 噪声滤波设置
        CMP_CR4[FAEN] = 0, T1INM1, T1INM0:00  4   clock
        CMP_CR4[FAEN] = 0, T1INM1, T1INM0:01  8   clock
        CMP_CR4[FAEN] = 0, T1INM1, T1INM0:10  16  clock
        CMP_CR4[FAEN] = 0, T1INM1, T1INM0:11  24  clock
        CMP_CR4[FAEN] = 1, T1INM1, T1INM0:00  32  clock
        CMP_CR4[FAEN] = 1, T1INM1, T1INM0:01  64  clock
        CMP_CR4[FAEN] = 1, T1INM1, T1INM0:10  96  clock
        CMP_CR4[FAEN] = 1, T1INM1, T1INM0:11  128 clock
    */
    ClrBit(CMP_CR4, FAEN);
    SetBit(TIM1_CR3, T1INM1 | T1INM0);
    /* Timer1 输入源选择
        T1TIS1,T1TIS0：00 GPIO输入，由HALL_CR[HALLSEL]决定是 P1.4/P1.6/P2.1或P0.2/P3.7/P3.6输入
        T1TIS1,T1TIS0：01 比较器(CPM0/1/2)的输出信号输入
        T1TIS1,T1TIS0：10 ADC计算结果符号翻转信号输入
    */
    SetReg(TIM1_CR3, T1TIS1 | T1TIS0, T1TIS0);
    /*  检测三相双沿 */
    SetBit(TIM1_DBR7, T1CPE2 | T1CPE1 | T1CPE0);
    TIM1_CR4 = 7;
    /* 定时器计数频率配置 */
    SetReg(TIM1_CR3, T1PSC2 | T1PSC1 | T1PSC0, T1PSC2 | T1PSC1 | T1PSC0);
    ClrBit(TIM1_CR0, T1FORC);
    TIM1__BARR = 60000;
    TIM1__BCNTR = 0;
    /* 配置中断优先级为次高 */
    SetReg(IP1, PTIM11 | PTIM10, PTIM11);
    /* 开启timer1 基本定时器溢出中断，位置检测中断 */
    SetReg(TIM1_IER, T1MAME | T1ADIE | T1BOIE | T1ROIE | T1WTIE | T1PDIE | T1BDIE, T1BOIE | T1PDIE);
    SetBit(TIM1_CR0, T1BCEN); // 基本定时器使能
    /*  清除中断标志 */
    ClrBit(TIM1_SR, T1ADIF | T1BOIF | T1ROIF | T1WTIF | T1PDIF | T1BDIF);
}

///**
//* @brief        Timer2 初始化函数
//* @date         2022-07-09
//*/
// static void Time2_Bemf_Init(void)
//{
//    /*  -------------------------------------------------------------------------------------------------
//        先停止计数，配置完寄存器后，最后启动计数
//        -------------------------------------------------------------------------------------------------*/
//    ClrBit(TIM2_CR1, T2CEN); // 0，停止计数；1,使能计数
//    /*  -------------------------------------------------------------------------------------------------
//        时钟分频设置(T2PSC)
//        000:cpuclk(24MHz)         001:cpuclk/2^1(12MHz)   010:cpuclk/2^2(6MHz)    011:cpuclk/2^3(3MHz)
//        100:cpuclk/2^4(1.5MHz)    101:cpuclk/2^5(750KHz)  110:cpuclk/2^6(375KHz)  111:cpuclk/2^7(187.5KHz)
//        -------------------------------------------------------------------------------------------------*/
//    SetReg(TIM2_CR0, T2PSC0 | T2PSC1 | T2PSC2, T2PSC0 | T2PSC1 | T2PSC2);
//    /*  -------------------------------------------------------------------------------------------------
//        /模式选择
//        T2MODE1，T2MODE0
//        00--输入Timer模式；01--输出模式
//        10--输入Count模式；11--QEP或者ISD模式
//        -------------------------------------------------------------------------------------------------*/
//    SetReg(TIM2_CR0, T2MOD0 | T2MOD1, T2MOD0);
//    /*  -------------------------------------------------------------------------------------------------
//        清除中断标志位
//        禁止PWM周期检测中断使能
//        -------------------------------------------------------------------------------------------------*/
//    ClrBit(TIM2_CR0, T2CES);                                // 清零脉冲计数器不使能
//    ClrBit(TIM2_CR1, T2IR | T2IF | T2IP);                   // 清零中断标志位
//    /*  -------------------------------------------------------------------------------------------------
//        配置周期值、比较值、计数值
//        禁止PWM周期检测中断使能
//        使能计数器上溢中断使能
//        -------------------------------------------------------------------------------------------------*/
//    TIM2__ARR  = 60000;                                    // TIM2 Period = 0.32s
//    TIM2__DR   = TIM2__ARR;
//    TIM2__CNTR = 0;
//    /*-----------启动计数------------------------------------------------*/
//    SetBit(TIM2_CR1, T2CEN);                                 //启动计数
//}

/**
 * @brief        基于Bemf的顺逆风检测初始化函数，电机状态切入 顺逆风检测状态时 运行一次
 * @date         2022-07-09
 */
void BEMFDetectInit(void)
{
    // BEMF检测前关闭mos输出
    memset(&mcBemf, 0, sizeof(BEMFDetect_TypeDef));
    mcBemf.FR = BEMF_FR_ERR;
    mcBemf.FRPre = BEMF_FR_ERR;
    mcBemf.BEMFSpeedBase = BEMFSpeedCalBase;

    mcBemf.FR_SET = mcFocCtrl.FR;

    /* -----使能比较器----- */
    Cmp_Bemf_Init();
    /* -----使能定时器1用于检测时间----- */
    Time1_Bemf_Init();
}

/**
 * @brief        bemf顺序为 5,1,3,2,6,4
 * @param        bemfStatus 当前bemf状态
 * @return       BEMF_FR_CCW 反转
 * @return       BEMF_FR_CW  正转
 * @return       BEMF_FR_ERR 错误
 * @date         2022-07-09
 */
static uint8 FR_Detect(uint8 bemfStatus, uint8 setFr)
{
    static uint8 temp_FR = 0;
    static uint8 temp_bemfStatusPre = 0;

    if (temp_bemfStatusPre == 0)
    {
        temp_bemfStatusPre = bemfStatus;
        temp_FR = BEMF_FR_ERR;
    }

    if (temp_bemfStatusPre != bemfStatus)
    {
        if (temp_bemfStatusPre == table_Bemf_CWPre[bemfStatus - 1])
        {

            if (setFr == CW) ///< 转向设置
            {
                temp_FR = BEMF_FR_CW;
            }
            else
            {
                temp_FR = BEMF_FR_CCW;
            }
        }
        else if (temp_bemfStatusPre == table_Bemf_CWNext[bemfStatus - 1])
        {
            if (setFr == CW) ///< 转向设置
            {
                temp_FR = BEMF_FR_CCW;
            }
            else
            {
                temp_FR = BEMF_FR_CW;
            }
        }
        else
        {
            temp_FR = BEMF_FR_ERR;
        }

        temp_bemfStatusPre = bemfStatus;
    }

    return temp_FR;
}

/**
 * @brief        void BemfProcess(void)基于Bemf的顺逆风检测，运行于Bemf检测比较器中断
 * @date         2022-07-09
 */
void BemfProcess(void)
{
    static uint8 temp_Cnt = 0;
    static uint8 temp_SumCnt = 0;
    uint16 temp_Sum = 0;
    int16 temp_speedCal;
    mcBemf.FRPre = mcBemf.FR;                            // 获取上一次的转向
    mcBemf.FR = FR_Detect(CMP_SR & 0x07, mcBemf.FR_SET); // 获取当前转向

    if (mcBemf.FR == BEMF_FR_CW) // 当前为正转
    {
        if (mcBemf.FRPre == mcBemf.FR)
        {
            if (temp_Cnt > 5) // 防溢出越界
            {
                temp_Cnt = 0;
            }

            mcBemf.SectorTime[temp_Cnt++] = TIM1__BCNTR;

            if (mcBemf.FRCount < 100)
            {
                mcBemf.FRCount++; //  连续两次均为正转则 正转计数+1
            }

            if (mcBemf.FRCount > 12)
            {
                temp_Sum = 0;

                for (temp_SumCnt = 0; temp_SumCnt < 6; temp_SumCnt++)
                {
                    temp_Sum += (mcBemf.SectorTime[temp_SumCnt] >> 3); // 防止溢出
                }

                mcBemf.PeriodTime = temp_Sum;
                mcBemf.Status = BEMF_FORWARD; // 连续12次均为正向转动,则bemf状态切换为 BEMF_FORWARD
            }
        }
        else
        {
            mcBemf.FRCount = 0;
            mcBemf.Status = BEMF_DETECTING;
        }
    }
    else if (mcBemf.FR == BEMF_FR_CCW)
    {
        if (mcBemf.FRPre == mcBemf.FR)
        {
            if (temp_Cnt > 5) // 防溢出越界
            {
                temp_Cnt = 0;
            }

            mcBemf.SectorTime[temp_Cnt++] = TIM1__BCNTR;

            if (mcBemf.FRCount > -100)
            {
                mcBemf.FRCount--; //  连续两次均为反转则 反转计数+1
            }

            if (mcBemf.FRCount < -12)
            {
                temp_Sum = 0;

                for (temp_SumCnt = 0; temp_SumCnt < 6; temp_SumCnt++)
                {
                    temp_Sum += (mcBemf.SectorTime[temp_SumCnt] >> 3); // 防止溢出
                }

                mcBemf.PeriodTime = temp_Sum;
                mcBemf.Status = BEMF_REVERSE;
            }
        }
        else
        {
            mcBemf.FRCount = 0;
            mcBemf.Status = BEMF_DETECTING;
        }
    }
    else
    {
        mcBemf.FRCount = 0; // 转向检测错误则清零转向计数
        mcBemf.Status = BEMF_DETECTING;
    }

    TIM1__BCNTR = 0;

    if (mcBemf.Status == BEMF_FORWARD || mcBemf.Status == BEMF_REVERSE) // 有效转向
    {
        if (mcBemf.PeriodTime < BEMFSpeedCalMinPeriod)
        {
            mcBemf.PeriodTime = BEMFSpeedCalMinPeriod; // 防止mcRsd.PeriodTime 太小导致计算出错
        }

        temp_speedCal = DivQ_L_MDU(mcBemf.BEMFSpeedBase >> 16, mcBemf.BEMFSpeedBase, mcBemf.PeriodTime);

        if (mcBemf.Status == BEMF_REVERSE)
        {
            mcBemf.BEMFSpeed = -temp_speedCal;
        }
        else
        {
            mcBemf.BEMFSpeed = temp_speedCal;
        }

        mcBemf.SpeedUpdate = 1;
    }

    if (mcBemf.HighSpdStart == 1) // 高速顺风启动
    {
        if ((CMP_SR & 0x07) == 0x01) // 03状态 对应90°启动
        {
            ClrBit(CMP_CR2, CMP0EN);  // 关闭比较器
            ClrBit(TIM1_CR0, T1BCEN); // 关闭定时器
            BEMFFOCCloseLoopStart();
            mcBemf.HighSpdStart = 2; // 启动完成
            // GP04 = ~GP04;
        }
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : BEMFFOCCloseLoopStart
    Description    : 闭环启动
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void BEMFFOCCloseLoopStart(void)
{
    /* FOC初始化 */
    FOC_Init();
    /* 启动电流、KP、KI */
    FOC_IDREF = ID_RUN_CURRENT;       // D轴启动电流
    FOC_IQREF = IQ_RUN_CURRENT;       // Q轴启动电流
    mcFocCtrl.IqRef = IQ_RUN_CURRENT; // Q轴启动电流
    FOC_DKP = _Q12(4.0);
    FOC_DKI = _Q15(0.5);
    FOC_QKP = _Q12(4.0);
    FOC_QKI = _Q15(0.5);
    FOC_EFREQACC = MOTOR_OMEGA_RAMP_ACC * 5;
    FOC_EFREQMIN = MOTOR_OMEGA_RAMP_MIN;
    FOC_EFREQHOLD = MOTOR_OMEGA_RAMP_END;
    SetBit(FOC_CR1, ANGM); // 估算模式
    ClrBit(FOC_CR1, RFAE); // 禁止强拉
    SetBit(FOC_CR1, EFAE); // 估算器强制输出
    FOC__EOME = mcBemf.BEMFSpeed;
#if (EstimateAlgorithm == SMO || EstimateAlgorithm == AO)
    FOC_EKP = OBSW_KP_GAIN_RUN4;
    FOC_EKI = OBSW_KI_GAIN_RUN4;
    mcFocCtrl.State_Count = ATO_RAMP_PERIOD << 1;
#elif (EstimateAlgorithm == PLL)
    FOC_EKP = OBSW_KP_GAIN_RUN4;
    FOC_EKI = OBSW_KI_GAIN_RUN4;
    mcFocCtrl.IqRef = IQ_RUN_CURRENT;
#endif // end    EstimateAlgorithm
    FOC_OMEKLPF = SPEED_KLPF;
    mcFocCtrl.CtrlMode = 0;
    /* 使能输出 */
    DRV_CMR |= 0x3F; // U、V、W相输出
    MOE = 1;
    EA = 1;
}
#endif
